#include "RTUFunctions.hpp"
#include "RTUSlave.hpp"

using namespace Modbus::Function;

#pragma region ReadCoils

/* 0x01, 读取线圈相关. */

ReadCoils::ReadCoils(uint16_t *buffer,unsigned int buffer_wsize,PortableStrategy *portable_strategy,RTUSlave *slave) : Function(slave){
    this->buffer = buffer;
    this->buffer_wsize = buffer_wsize;
    this->portable_strategy = portable_strategy;
}

unsigned int ReadCoils::getFunctionCode(void){
    return ReadCoils::FUNCTION_CODE;
}

int ReadCoils::parse(void *pdu,unsigned int size,bool broadcast){
    if ( broadcast == true ){
        /* Read Coils not support broadcast mode. */
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( this->slave == nullptr ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( this->portable_strategy == nullptr ){
        return RTUSlave::Result::NO_PORTABLE_STRATEGY;
    }
    uint8_t *u8_pdu = reinterpret_cast<uint8_t*>(pdu);
    uint8_t *u8_buffer = reinterpret_cast<uint8_t*>(this->buffer);
    uint8_t function_code = 0x00;
    if ( size != 5 ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    function_code = u8_pdu[0];
    if ( function_code != ReadCoils::FUNCTION_CODE ){
        return RTUSlave::Result::NO_FUNCTION;
    }
    uint16_t start_address = 0x0000;
    uint16_t bit_count = 0x0000;
    start_address |= (u8_pdu[1] << 8);
    start_address |= (u8_pdu[2]);
    bit_count |= (u8_pdu[3] << 8);
    bit_count |= (u8_pdu[4]);
    /* Calculate response buffer size require. */
    unsigned int data_field_size = (bit_count % 8 == 0) ? (bit_count / 8) : ((bit_count / 8) + 1);
    if ( (this->buffer_wsize * 2) < (data_field_size + 2) ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    for ( int n_word = 0; n_word < this->buffer_wsize; n_word++ ){
        this->buffer[n_word] = 0x0000;
    }
    for ( int n_bit = 0; n_bit < bit_count; n_bit++ ){
        uint8_t bit_buffer = 0x00;
        int ret = this->portable_strategy->getBit(&bit_buffer,start_address + n_bit);
        if ( ret != RTUSlave::Result::SUCCESS ){
            return ret;
        }
        if ( bit_buffer != 0 ){
            u8_buffer[2 + (n_bit / 8)] |= (0x01 << (n_bit % 8));
        }else{
            u8_buffer[2 + (n_bit / 8)] &= ~(0x01 << (n_bit % 8));
        }
    }
    u8_buffer[0] = ReadCoils::FUNCTION_CODE;
    u8_buffer[1] = data_field_size;

    return this->slave->sendResponse(u8_buffer,(data_field_size + 2));
}

#pragma endregion

#pragma region ReadDiscreteInputs

/* 0x02. 读取离散量相关. */

ReadDiscreteInputs::ReadDiscreteInputs(uint16_t *buffer,unsigned int buffer_wsize,PortableStrategy *portable_strategy,RTUSlave *slave) : Function(slave){
    this->buffer = buffer;
    this->buffer_wsize = buffer_wsize;
    this->portable_strategy = portable_strategy;
}

unsigned int ReadDiscreteInputs::getFunctionCode(void){
    return ReadDiscreteInputs::FUNCTION_CODE;
}

int ReadDiscreteInputs::parse(void *pdu,unsigned int size,bool broadcast){
    if ( broadcast == true ){
        /* Read Discrete Inputs not support broadcast mode. */
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( this->slave == nullptr ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( this->portable_strategy == nullptr ){
        return RTUSlave::Result::NO_PORTABLE_STRATEGY;
    }
    uint8_t *u8_pdu = reinterpret_cast<uint8_t*>(pdu);
    uint8_t *u8_buffer = reinterpret_cast<uint8_t*>(this->buffer);
    uint8_t function_code = 0x00;
    if ( size != 5 ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    function_code = u8_pdu[0];
    if ( function_code != ReadDiscreteInputs::FUNCTION_CODE ){
        return RTUSlave::Result::NO_FUNCTION;
    }
    uint16_t start_address = 0x0000;
    uint16_t bit_count = 0x0000;
    start_address |= (u8_pdu[1] << 8);
    start_address |= (u8_pdu[2]);
    bit_count |= (u8_pdu[3] << 8);
    bit_count |= (u8_pdu[4]);
    /* Calculate response buffer size require. */
    unsigned int data_field_size = (bit_count % 8 == 0) ? (bit_count / 8) : ((bit_count / 8) + 1);
    if ( (this->buffer_wsize * 2) < (data_field_size + 2) ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    for ( int n_word = 0; n_word < this->buffer_wsize; n_word++ ){
        this->buffer[n_word] = 0x0000;
    }
    for ( int n_bit = 0; n_bit < bit_count; n_bit++ ){
        uint8_t bit_buffer = 0x00;
        int ret = this->portable_strategy->getBit(&bit_buffer,start_address + n_bit);
        if ( ret != RTUSlave::Result::SUCCESS ){
            return ret;
        }
        if ( bit_buffer != 0 ){
            u8_buffer[2 + (n_bit / 8)] |= (0x01 << (n_bit % 8));
        }else{
            u8_buffer[2 + (n_bit / 8)] &= ~(0x01 << (n_bit % 8));
        }
    }
    u8_buffer[0] = ReadDiscreteInputs::FUNCTION_CODE;
    u8_buffer[1] = data_field_size;

    return this->slave->sendResponse(u8_buffer,(data_field_size + 2));
    return RTUSlave::Result::SUCCESS;
}

#pragma endregion

#pragma region 

/* 0x03. 读取保持寄存器相关. */

ReadHoldingRegister::ReadHoldingRegister(uint16_t *buffer,unsigned int buffer_wsize,PortableStrategy *portable_strategy,RTUSlave *slave) : Function(slave){
    this->buffer = buffer;
    this->buffer_wsize = buffer_wsize;
    this->portable_strategy = portable_strategy;
}

unsigned int ReadHoldingRegister::getFunctionCode(void){
    return ReadHoldingRegister::FUNCTION_CODE;
}

int ReadHoldingRegister::parse(void *pdu,unsigned int size,bool broadcast){
    if ( broadcast == true ){
        /* Read Holding Register not support broadcast mode. */
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( this->slave == nullptr ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( this->portable_strategy == nullptr ){
        return RTUSlave::Result::NO_PORTABLE_STRATEGY;
    }
    uint8_t *u8_pdu = reinterpret_cast<uint8_t*>(pdu);
    uint8_t *u8_buffer = reinterpret_cast<uint8_t*>(this->buffer);
    uint8_t function_code = 0x00;
    if ( size != 5 ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    function_code = u8_pdu[0];
    if ( function_code != ReadHoldingRegister::FUNCTION_CODE ){
        return RTUSlave::Result::NO_FUNCTION;
    }
    uint16_t start_address = 0x0000;
    uint16_t register_count = 0x0000;
    start_address |= (u8_pdu[1] << 8);
    start_address |= (u8_pdu[2]);
    register_count |= (u8_pdu[3] << 8);
    register_count |= (u8_pdu[4]);
    /* Calculate response buffer size require. */
    if ( this->buffer_wsize < register_count + 1 ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    for ( int n_word = 0; n_word < register_count; n_word++ ){
        uint16_t word = 0x0000;
        int ret = this->portable_strategy->getWord(&word,start_address + n_word);
        if ( ret != RTUSlave::Result::SUCCESS ){
            return ret;
        }
        u8_buffer[2 + n_word * 2] = (word >> 8) & 0xff;
        u8_buffer[2 + n_word * 2 + 1] = (word & 0xff);
    }
    u8_buffer[0] = ReadHoldingRegister::FUNCTION_CODE;
    u8_buffer[1] = register_count * 2;

    return this->slave->sendResponse(u8_buffer,(register_count * 2 + 2));
}

#pragma endregion

#pragma region 

/* 0x04. 读取输入寄存器相关. */

ReadInputRegister::ReadInputRegister(uint16_t *buffer,unsigned int buffer_wsize,PortableStrategy *portable_strategy,RTUSlave *slave) : Function(slave){
    this->buffer = buffer;
    this->buffer_wsize = buffer_wsize;
    this->portable_strategy = portable_strategy;
}

unsigned int ReadInputRegister::getFunctionCode(void){
    return ReadInputRegister::FUNCTION_CODE;
}

int ReadInputRegister::parse(void *pdu,unsigned int size,bool broadcast){
    if ( broadcast == true ){
        /* Read Input Register not support broadcast mode. */
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( this->slave == nullptr ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( this->portable_strategy == nullptr ){
        return RTUSlave::Result::NO_PORTABLE_STRATEGY;
    }
    uint8_t *u8_pdu = reinterpret_cast<uint8_t*>(pdu);
    uint8_t *u8_buffer = reinterpret_cast<uint8_t*>(this->buffer);
    uint8_t function_code = 0x00;
    if ( size != 5 ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    function_code = u8_pdu[0];
    if ( function_code != ReadInputRegister::FUNCTION_CODE ){
        return RTUSlave::Result::NO_FUNCTION;
    }
    uint16_t start_address = 0x0000;
    uint16_t register_count = 0x0000;
    start_address |= (u8_pdu[1] << 8);
    start_address |= (u8_pdu[2]);
    register_count |= (u8_pdu[3] << 8);
    register_count |= (u8_pdu[4]);
    /* Calculate response buffer size require. */
    if ( this->buffer_wsize < register_count + 1 ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    for ( int n_word = 0; n_word < register_count; n_word++ ){
        uint16_t word = 0x0000;
        int ret = this->portable_strategy->getWord(&word,start_address + n_word);
        if ( ret != RTUSlave::Result::SUCCESS ){
            return ret;
        }
        u8_buffer[2 + n_word * 2] = (word >> 8) & 0xff;
        u8_buffer[2 + n_word * 2 + 1] = (word & 0xff);
    }
    u8_buffer[0] = ReadInputRegister::FUNCTION_CODE;
    u8_buffer[1] = register_count * 2;

    return this->slave->sendResponse(u8_buffer,(register_count * 2 + 2));
}

#pragma endregion

#pragma region 

/* 0x05. 写单个线圈相关. */

WriteCoil::WriteCoil(uint16_t *buffer,unsigned int buffer_wsize,PortableStrategy *portable_strategy,RTUSlave *slave) : Function(slave){
    this->buffer = buffer;
    this->buffer_wsize = buffer_wsize;
    this->portable_strategy = portable_strategy;
}

unsigned int WriteCoil::getFunctionCode(void){
    return WriteCoil::FUNCTION_CODE;
}

int WriteCoil::parse(void *pdu,unsigned int size,bool broadcast){
    if ( this->slave == nullptr ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( this->portable_strategy == nullptr ){
        return RTUSlave::Result::NO_PORTABLE_STRATEGY;
    }
    uint8_t *u8_pdu = reinterpret_cast<uint8_t*>(pdu);
    uint8_t *u8_buffer = reinterpret_cast<uint8_t*>(this->buffer);
    (void)u8_buffer;
    uint8_t function_code = 0x00;
    if ( size != 5 ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    function_code = u8_pdu[0];
    if ( function_code != WriteCoil::FUNCTION_CODE ){
        return RTUSlave::Result::NO_FUNCTION;
    }
    uint16_t address = 0x0000;
    uint16_t value = 0x0000;
    address |= (u8_pdu[1] << 8);
    address |= (u8_pdu[2]);
    value |= (u8_pdu[3] << 8);
    value |= (u8_pdu[4]);
    uint8_t bit_buffer = (value == 0xff00) ? 1 : 0;
    int ret = this->portable_strategy->setBit(&bit_buffer,address);
    if ( ret != RTUSlave::Result::SUCCESS ){
        return ret;
    }

    if ( broadcast != true ){
        this->slave->sendResponse(pdu,size);
    }

    return RTUSlave::Result::SUCCESS;
}

#pragma endregion

#pragma region 

/* 0x06. 写单个寄存器相关. */

WriteHoldingRegister::WriteHoldingRegister(uint16_t *buffer,unsigned int buffer_wsize,PortableStrategy *portable_strategy,RTUSlave *slave) : Function(slave){
    this->buffer = buffer;
    this->buffer_wsize = buffer_wsize;
    this->portable_strategy = portable_strategy;
}

unsigned int WriteHoldingRegister::getFunctionCode(void){
    return WriteHoldingRegister::FUNCTION_CODE;
}

int WriteHoldingRegister::parse(void *pdu,unsigned int size,bool broadcast){
    if ( this->slave == nullptr ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( this->portable_strategy == nullptr ){
        return RTUSlave::Result::NO_PORTABLE_STRATEGY;
    }
    uint8_t *u8_pdu = reinterpret_cast<uint8_t*>(pdu);
    uint8_t *u8_buffer = reinterpret_cast<uint8_t*>(pdu);
    (void)u8_buffer;
    uint8_t function_code = 0x00;
    if ( size != 5 ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    function_code = u8_pdu[0];
    if ( function_code != WriteHoldingRegister::FUNCTION_CODE ){
        return RTUSlave::Result::NO_FUNCTION;
    }
    uint16_t address = 0x0000;
    uint16_t value = 0x0000;
    address |= (u8_pdu[1] << 8);
    address |= (u8_pdu[2]);
    value |= (u8_pdu[3] << 8);
    value |= (u8_pdu[4]);

    int ret = this->portable_strategy->setWord(&value,address);
    if ( ret != RTUSlave::Result::SUCCESS ){
        return ret;
    }

    if ( broadcast != true ){
        this->slave->sendResponse(pdu,size);
    }

    return RTUSlave::Result::SUCCESS;
}

#pragma endregion

#pragma region 

/* 0x0F. 写多个线圈相关. */

WriteMultipleCoils::WriteMultipleCoils(uint16_t *buffer,unsigned int buffer_wsize,PortableStrategy *portable_strategy,RTUSlave *slave) : Function(slave){
    this->buffer = buffer;
    this->buffer_wsize = buffer_wsize;
    this->portable_strategy = portable_strategy;
}

unsigned int WriteMultipleCoils::getFunctionCode(void){
    return WriteMultipleCoils::FUNCTION_CODE;
}

int WriteMultipleCoils::parse(void *pdu,unsigned int size,bool broadcast){
    if ( this->slave == nullptr ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( this->portable_strategy == nullptr ){
        return RTUSlave::Result::NO_PORTABLE_STRATEGY;
    }
    uint8_t *u8_pdu = reinterpret_cast<uint8_t*>(pdu);
    uint8_t *u8_buffer = reinterpret_cast<uint8_t*>(this->buffer);
    uint8_t function_code = 0x00;
    if ( size < 6 ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    function_code = u8_pdu[0];
    if ( function_code != WriteMultipleCoils::FUNCTION_CODE ){
        return RTUSlave::Result::NO_FUNCTION;
    }
    uint16_t start_address = 0x0000;
    uint16_t coil_count = 0x0000;
    uint8_t byte_count = 0x00;
    start_address |= (u8_pdu[1] << 8);
    start_address |= (u8_pdu[2]);
    coil_count |= (u8_pdu[3] << 8);
    coil_count |=(u8_pdu[4]);
    byte_count = u8_pdu[5];
    if ( byte_count != size - 6 ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( byte_count * 8 < coil_count ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    for ( int n_coil = 0; n_coil < coil_count; n_coil++ ){
        uint8_t bit_buffer = 0x00;
        if ( u8_pdu[6 + (n_coil / 8)] & (0x01 << (n_coil % 8)) != 0x00 ){
            bit_buffer = 1;
        }else{
            bit_buffer = 0;
        }
        int ret = this->portable_strategy->setBit(&bit_buffer,start_address + n_coil);
        if ( ret != RTUSlave::Result::SUCCESS ){
            return ret;
        }
    }
    if ( broadcast != true ){
        /* Construct response. */
        if ( this->buffer_wsize < 3 ){
            return RTUSlave::Result::DEVICE_FAULT;
        }
        u8_buffer[0] = WriteMultipleCoils::FUNCTION_CODE;
        u8_buffer[1] = (start_address >> 8) & 0xff;
        u8_buffer[2] = (start_address & 0xff);
        u8_buffer[3] = (coil_count >> 8) & 0xff;
        u8_buffer[4] = (coil_count & 0xff);
        return this->slave->sendResponse(u8_buffer,5);
    }

    return RTUSlave::Result::SUCCESS;
}

#pragma endregion

#pragma region 

/* 0x10. 写多个寄存器. */

WriteMultipleHoldingRegisters::WriteMultipleHoldingRegisters(uint16_t *buffer,unsigned int buffer_wsize,PortableStrategy *portable_strategy,RTUSlave *slave) : Function(slave){
    this->buffer = buffer;
    this->buffer_wsize = buffer_wsize;
    this->portable_strategy = portable_strategy;
}

unsigned int WriteMultipleHoldingRegisters::getFunctionCode(void){
    return WriteMultipleHoldingRegisters::FUNCTION_CODE;
}

int WriteMultipleHoldingRegisters::parse(void *pdu,unsigned int size,bool broadcast){
    if ( this->slave == nullptr ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( this->portable_strategy == nullptr ){
        return RTUSlave::Result::NO_PORTABLE_STRATEGY;
    }
    uint8_t *u8_pdu = reinterpret_cast<uint8_t*>(pdu);
    uint8_t *u8_buffer = reinterpret_cast<uint8_t*>(this->buffer);
    uint8_t function_code = 0x00;
    if ( size < 6 ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    function_code = u8_pdu[0];
    if ( function_code != WriteMultipleHoldingRegisters::FUNCTION_CODE ){
        return RTUSlave::Result::NO_FUNCTION;
    }
    uint16_t start_address = 0x0000;
    uint16_t reg_count = 0x0000;
    uint8_t byte_count = 0x00;
    start_address |= (u8_pdu[1] << 8);
    start_address |= (u8_pdu[2]);
    reg_count |= (u8_pdu[3] << 8);
    reg_count |=(u8_pdu[4]);
    byte_count = u8_pdu[5];
    if ( byte_count != size - 6 ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    if ( byte_count < reg_count * 2 ){
        return RTUSlave::Result::DEVICE_FAULT;
    }
    for ( int n_reg = 0; n_reg < reg_count; n_reg++ ){
        uint16_t reg_buffer = 0x0000;
        reg_buffer |= (u8_pdu[6 + 2 * n_reg ] << 8);
        reg_buffer |= (u8_pdu[6 + 2 * n_reg + 1]);
        int ret = this->portable_strategy->setWord(&reg_buffer,start_address + n_reg);
        if ( ret != RTUSlave::Result::SUCCESS ){
            return ret;
        }
    }

    if ( broadcast != true ){
        /* Construct response. */
        if ( this->buffer_wsize < 3 ){
            return RTUSlave::Result::DEVICE_FAULT;
        }
        u8_buffer[0] = WriteMultipleHoldingRegisters::FUNCTION_CODE;
        u8_buffer[1] = (start_address >> 8) & 0xff;
        u8_buffer[2] = (start_address & 0xff);
        u8_buffer[3] = (reg_count >> 8) & 0xff;
        u8_buffer[4] = (reg_count & 0xff);
        return this->slave->sendResponse(u8_buffer,5);
    }

    return RTUSlave::Result::SUCCESS;
}

#pragma endregion


